#include "../../include/robot/AlignAction.h"

AlignAction::AlignAction(double degree,VisionController *visione):ArAction("AlignAction", "Si allinea alla palla") {

	this->degree = degree;
	this->aligned = false;
	this->visione = visione;
}

AlignAction::~AlignAction() {

}

ArActionDesired* AlignAction::fire(ArActionDesired currentDesired){

	myDesired.reset();
	double toRotate = -1;
	int range = 1;
	if(degree<0)
		toRotate=1;

	if(degree>=-range && degree<=range){
		visione->alza();
		//visione->elaborateImage();
		double distance =0;
		double beta;
		double gradi;
		float angoloPorta;
		float angoloPalla;

		if(this->aligned==false){
			distance = elaborateDistance(visione->getGoalDepth(),visione->getBallDepth());
			distance = distance+150;
			cout <<"distanza: "<<distance<<"\n";
			beta = elaborateBeta(distance,visione->getBallDepth());
			gradi = -1;
			angoloPorta = visione->getGoalAngle();
			angoloPalla = visione->getBallAngle();
			if(angoloPorta<angoloPalla)
				gradi = -beta;
			else
				gradi = beta;

			myRobot->setDeltaHeading(gradi/8);
			std::cout<<"beta: "<<beta<<"\n"<<"distanza: "<<distance<<"\n";
			myRobot->setVel(100);

			myRobot->move(distance);
			this->aligned = true;
		}

		if(myRobot->isMoveDone(distance) && aligned==true){
			cout<<"siamo nell'if dell'allineamento!\n";
			visione->azzera();
			if(gradi>0){
				myRobot->setDeltaHeading(-1);
			}else if(gradi<0){
				myRobot->setDeltaHeading(1);
			}
		}
	}else{
		degree= degree+toRotate;
		myRobot->setDeltaHeading(toRotate);
	}

	return &myDesired;

}

double AlignAction::elaborateBeta(float d, float profPalla){
	double beta;
	//double arcArg = (-pow(S,2)+pow(profPalla,2)+pow(d,2))/(2*d*(int)(profPalla));
	double arcArg = (pow(d,2)+ pow(profPalla,2) -pow(S,2))/ (2*d*profPalla);
	if(arcArg>1)
		arcArg=1;
	else if(arcArg<1)
		arcArg=-1;
	beta = acos(arcArg);
	beta = (beta*180)/3.14;

	return beta;
}
double AlignAction::elaborateDistance(float profPorta,float profPalla){
	double d;
	double arcArg = (-pow(profPorta,2)+pow(M,2)+pow(profPalla,2))/(2*M*profPalla);

	if(arcArg>1)
		arcArg = 1;
	else if(arcArg<-1)
		arcArg=-1;
	double arcos = acos(arcArg);
	std::cout <<"ARCOS: "<<arcos<<"\n";
	d = sqrt(pow(S,2)+pow(profPalla,2)-2*S*profPalla*cos(180-arcos));
	return d;
}

